#include "solar_tracking.h"

#include <stdlib.h>
#include <stm32f1xx_hal.h>
#include "adc.h"
#include "data_queue.h"
#include "data_kalman_filter.h"
#include "rod_motor.h"
#include "stepping_motor.h"

#define HORIZONTAL_LIMIT_VALUE      5
#define VERTICAL_LIMIT_VALUE        30

#define PID_HORIZONTAL_KP 0.03
#define PID_HORIZONTAL_KI 0.01
#define PID_HORIZONTAL_KD 0

volatile uint16_t adc_values[4];

DATA_KalmanFilter horizontal_kf;

DATA_Queue horizontal_errors;
DATA_Queue vertical_errors;

inline ElementType INTERNAL_LimitRange(const ElementType num, const ElementType limit) {
    if (num < -limit || limit < num) return limit;
    return num;
}

void SOLAR_TRACKING_Init() {
    const float initial_x = 0;
    const float initial_predict_error_cov = 1000;
    const float process_noise_cov = 0.01f;
    const float measure_noise_cov = 25.0f;
    DATA_KalmanFilter_Init(&horizontal_kf, initial_x, initial_predict_error_cov, process_noise_cov, measure_noise_cov);

    DATA_QueueInit(&horizontal_errors);
    DATA_QueueInit(&vertical_errors);

    HAL_ADCEx_Calibration_Start(&hadc1);
    // ReSharper disable once CppRedundantCastExpression
    HAL_ADC_Start_DMA(&hadc1, (uint32_t*)adc_values, sizeof(adc_values) / sizeof(uint16_t));

    HAL_ADC_PollForConversion(&hadc1, HAL_MAX_DELAY);
}

void SOLAR_TRACKING_Collect() {
    const uint16_t up       = (adc_values[0] + adc_values[1]) / 2;
    const uint16_t down     = (adc_values[2] + adc_values[3]) / 2;
    const uint16_t left     = (adc_values[0] + adc_values[3]) / 2;
    const uint16_t right    = (adc_values[1] + adc_values[2]) / 2;

    const int32_t horizontal_error  = left - right;
    const int32_t vertical_error    = down - up;

    DATA_KalmanFilter_Predict(&horizontal_kf);
    DATA_KalmanFilter_Update(&horizontal_kf, (float)horizontal_error);

    DATA_QueueEnqueue(&horizontal_errors, (int32_t)horizontal_kf.x);
    DATA_QueueEnqueue(&vertical_errors, vertical_error);
}

void SOLAR_TRACKING_Follow() {
    const int32_t horizontal_error = DATA_QueueGet(&horizontal_errors, -1);
    const int32_t vertical_error = DATA_QueueGet(&vertical_errors, -1);

    const double horizontal_pid =
        PID_HORIZONTAL_KP * horizontal_error +
        PID_HORIZONTAL_KI * INTERNAL_LimitRange(DATA_QueueSum(&horizontal_errors), 1000) +
        PID_HORIZONTAL_KD * (horizontal_error - DATA_QueueGet(&horizontal_errors, -2));

    if (horizontal_pid < -HORIZONTAL_LIMIT_VALUE ||  HORIZONTAL_LIMIT_VALUE < horizontal_pid) {
        STEPPING_MOTOR_TurnAngle((uint32_t)(3000 / horizontal_pid), horizontal_pid < 0 ? -3.6f : 3.6f);
    }

    if (vertical_error < -VERTICAL_LIMIT_VALUE) {
        ROD_MOTOR_Stretch();
    } else if (vertical_error > VERTICAL_LIMIT_VALUE) {
        ROD_MOTOR_Shrink();
    } else {
        ROD_MOTOR_Stop();
    }
}
